#include <opencv2/opencv.hpp>
#include "MoveSenseCamera.h"

using namespace movesense;

MoveSenseCamera *camera;

int main()
{
	bool rectify_sw = true;

	camera = new MoveSenseCamera();
	if (!camera->IsAvailable())
	{
		cout << "No camera connected..." << endl;
		return 0;
	}

	//CAMERA_LR  CAMERA_LD  CAMERA_LRD
	int res = camera->SetMode(CAMERA_LRD);
	if (res != MS_SUCCESS)
	{
		cout << "The mode is not supported..." << endl;
		return 0;
	}

	if (camera->Open() != MS_SUCCESS)
	{
		cout << "Open camera failed!" << endl;
		return 0;
	}
	cout << "Open camera ok!" << endl;

	res = camera->StartStream();
	camera->SetStereoRectify(rectify_sw);

	//Get camera setting
	Resolution resolution = camera->GetResolution();
	int w = resolution.width;
	int h = resolution.height;
	int len = camera->GetImageDataLength();

	unsigned char *data = new unsigned char[len];
	cv::Mat left(h,w,CV_8UC1),right(h,w,CV_8UC1),disparity(h,w,CV_8UC1);

	int frameCnt = 0;
	double time1=0, time2=0, time=0;

	int open_index=0;

	while (1)
	{
		camera->StopStream();
		camera->Close();

		if (camera->Open() != MS_SUCCESS)
		{
			cout << "Open camera failed!" << endl;
			return 0;
		}
		cout << "Open camera ok!" << endl;

		camera->StartStream();

		open_index++;
		cout << "open_index:" << open_index << endl;

		res = camera->GetImageData(left.data, right.data, disparity.data, len);

		if(res == MS_SUCCESS)
		{
			frameCnt++;

			if (frameCnt == 50)
			{
				time2 = cv::getTickCount();
				time = (time2-time1)/cv::getTickFrequency();
				cout << "fps: " << (frameCnt)/time << endl;

				frameCnt = 0;
				time1 = time2;
			}

			cv::imshow("left", left);
			cv::imshow("right", right);  
			cv::imshow("disp", disparity);

			int key = cv::waitKey(5);
			if (key == 'q' || key == 'Q')
			{
				cv::destroyAllWindows();
				break;
			}
		}
		else if (res == MS_TIMEOUT)
		{
			cout << "GetImageData: timeout" << endl;
		}
		else
		{
			cout << "GetImageData: failed" << endl;
		}
	}

	camera->StopStream();
	camera->Close();
	delete camera;
	delete data;

	return MS_SUCCESS;
}
